

////////////////  宏定义 勿改动 //////////////////
#define DEMO_MODE_CYCLE 0  //demo 循环播放 
#define DEMO_MODE_ONCE  1  //demo 只播放一次
#define ABSA            0  // 动作描述中角度信息的类型: 绝对值 (无视舵机当前角度，直接指定舵机角度)
#define RELA            1  // 动作描述中角度信息的类型: 相对值 (以舵机当前角度为基准偏移指定角度)
#define SERVO_SUM_MAX   9  // 参与动作的舵机数量
////////////////  功能配置宏定义 充分理解含义后再改动 //////////////////
#define SERVO_SUM       9
#define DEMO_MODE       DEMO_MODE_ONCE
#define delay_ms(ms)    delay((ms))

//舵机动作描述, 舵机动作库的基本单元
typedef struct _servo_motion_
{
  int16_t   angle;  // 角度调整信息 单位: deg // 
  int8_t    type;   // ABSA or RELA
  int16_t   time;   // 指定动作完成时间
}ServoMotion;

//舵机描述, 用以管理记录某个舵机的工作状态
typedef struct _servo_
{
  uint8_t id;     // 舵机ID 
  int16_t angle;  // 舵机当前角度 单位: 度
  int16_t adjust; // 角度偏移校正值
}ServoObj;

//舵机群复杂动作描述基本单元
typedef struct _servo_animation_unit_
{
  int16_t  mid;    //选定的基础动作ID 即在动作库中的坐标
  int16_t  delay;  //给舵机群发出这套动作指令之后(不等动作完成)，需要等待多久才能进行下一个基础动作 单位 毫秒
  uint8_t  speed;  //变速因子(25~50)，基础动作库中的每个动作都是1秒完成90度的转动，实际执行动作时可以适当加快
                   //speed=50时完成动作耗时1000ms，speed=49耗时980ms , 40耗时800ms 以此类推
                   //但舵机转动速度总是有个上限的，所以speed也有个下限，具体我没测过，暂定为25吧，即500ms完成动作
}AnimationUnit;

//舵机群基础动作库  (存储在FLASH中)
const ServoMotion servo_motion_lib[][SERVO_SUM_MAX] PROGMEM = 
{
//servoID:    1#               2#               3#               4#               5#               6#               7#               8#               9#   
//--------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//0#动作: 1#舵机角度归零,耗时1秒 其他不变    
      {{  0,ABSA, 1000},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0}, },
//1#动作: 2#舵机角度归零,耗时1秒 其他不变       
      {{  0,RELA,    0},{  0,ABSA, 1000},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0}, },
//2#
      {{  0,RELA,    0},{  0,RELA,    0},{  0,ABSA, 1000},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0}, },
//3#
      {{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,ABSA, 1000},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0}, },
//4#
      {{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,ABSA, 1000},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0}, },
//5#
      {{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,ABSA, 1000},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0}, },
//6#
      {{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,ABSA, 1000},{  0,RELA,    0},{  0,RELA,    0}, },
//7#
      {{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,ABSA, 1000},{  0,RELA,    0}, },
//8#
      {{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,ABSA, 1000}, },
//9#动作: 9#舵机顺时针+90度 耗时1秒 其他不变
      {{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{ 90,RELA, 1000}, },
//10#动作: 8#舵机顺时针+90度 耗时1秒 其他不变
      {{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{ 90,RELA, 1000},{  0,RELA,    0}, },
//11#
      {{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{ 90,RELA, 1000},{  0,RELA,    0},{  0,RELA,    0}, },
//12#
      {{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{ 90,RELA, 1000},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0}, },
//13#
      {{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{ 90,RELA, 1000},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0}, },
//14#
      {{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{ 90,RELA, 1000},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0}, },
//15#
      {{  0,RELA,    0},{  0,RELA,    0},{ 90,RELA, 1000},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0}, },
//16#
      {{  0,RELA,    0},{ 90,RELA, 1000},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0}, },
//17#
      {{ 90,RELA, 1000},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0}, },
//18# 所有舵机顺时针+90度 耗时1秒
      {{ 90,RELA, 1000},{ 90,RELA, 1000},{ 90,RELA, 1000},{ 90,RELA, 1000},{ 90,RELA, 1000},{ 90,RELA, 1000},{ 90,RELA, 1000},{ 90,RELA, 1000},{ 90,RELA, 1000}, },
//19# 所有舵机逆时针-90度 耗时1秒
      {{-90,RELA, 1000},{-90,RELA, 1000},{-90,RELA, 1000},{-90,RELA, 1000},{-90,RELA, 1000},{-90,RELA, 1000},{-90,RELA, 1000},{-90,RELA, 1000},{-90,RELA, 1000}, },
//20#动作: 1#舵机逆时针-90度 其他不变
      {{-90,RELA, 1000},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0}, },
//21#动作: 2#舵机逆时针-90度 其他不变
      {{  0,RELA,    0},{-90,RELA, 1000},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0}, },
//22# 
      {{  0,RELA,    0},{  0,RELA,    0},{-90,RELA, 1000},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0}, },
//23# 
      {{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{-90,RELA, 1000},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0}, },
//24# 
      {{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{-90,RELA, 1000},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0}, },
//25# 
      {{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{-90,RELA, 1000},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0}, },
//26# 
      {{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{-90,RELA, 1000},{  0,RELA,    0},{  0,RELA,    0}, },
//27# 
      {{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{-90,RELA, 1000},{  0,RELA,    0}, },
//28# 
      {{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{-90,RELA, 1000}, },
//29#动作: 1#舵机角度180,耗时1秒 其他不变    
      {{180,ABSA, 1000},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0}, },
//30#动作: 2#舵机角度180,耗时1秒 其他不变       
      {{  0,RELA,    0},{180,ABSA, 1000},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0}, },
//31#
      {{  0,RELA,    0},{  0,RELA,    0},{180,ABSA, 1000},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0}, },
//32#
      {{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{180,ABSA, 1000},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0}, },
//33#
      {{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{180,ABSA, 1000},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0}, },
//34#
      {{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{180,ABSA, 1000},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0}, },
//35#
      {{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{180,ABSA, 1000},{  0,RELA,    0},{  0,RELA,    0}, },
//36#
      {{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{180,ABSA, 1000},{  0,RELA,    0}, },
//37#
      {{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{  0,RELA,    0},{180,ABSA, 1000}, },
};

//舵机群动作序列 (存储在FLASH中)
//将各种基础动作通过不同的延时粘合在一起，构成复杂的动作序列
const AnimationUnit servo_demo[] PROGMEM = 
{
  /////////////////////////////   位置初始化 全部躺下   //////////////////////////////////////
  ////全部角度归零(横躺) 标准速度
  { 0,    0,100}, { 1,    0,100}, { 2,    0,100}, { 3,    0,100}, { 4,    0,100}, { 5,    0,100}, { 6,    0,100}, { 7,    0,100}, { 8, 2000,100}, 
  ///////////////////////////////////////////////////////////////////
  ////反序挨个竖起 110% speed
  { 9,  900, 90}, {10,  900, 90}, {11,  900, 90}, {12,  900, 90}, {13,  900, 90}, {14,  900, 90}, {15,  900, 90}, {16,  900, 90}, {17, 1500, 90}, 
  // ////顺时针全部躺下 120% 
  {18, 1500, 80},
  // ////正序挨个竖起 130%
  {20,  700, 70}, {21,  700, 70}, {22,  700, 70}, {23,  700, 70}, {24,  700, 70}, {25,  700, 70}, {26,  700, 70}, {27,  700, 70}, {28, 1500,70}, 
  // ///////////////////////////////////////////////////////////////////
  // ////逆时针正序挨个躺下 但不等前一个完全躺下 下一个就要开始动作 标准速度
  {20,  130, 60}, {21,  130, 60}, {22,  130, 60}, {23,  130, 60}, {24,  130, 60}, {25,  130, 60}, {26,  130, 60}, {27,  130, 60}, {28, 1500, 60}, 
  // ////正序 奇数舵机先竖起 反序偶数舵机后竖起
  {17,  800, 80}, {15,  800, 80}, {13,  800, 80}, {11,  800, 80}, { 9,  800, 80}, 
  {10,  800, 80}, {12,  800, 80}, {14,  800, 80}, {16,  800, 80},
  // ////反序两两一组依次顺时针躺下
  {29,    0, 50}, {30,  600, 50}, {31,    0, 80}, {32, 1000, 80}, {33,    0,110}, {34, 1200,110}, {35,    0, 150}, {36,    0, 150}, {37,2500,150}, 
  // ////反序逆时针起来且执行速度越来越快 
  {28, 1000,100}, {27,  960, 96}, {26,  920, 92}, {25,  880, 88}, {24,  840, 84}, {23,  800, 80}, {22,  720, 72}, {21,  600, 60}, {20, 2500, 50},
  // // ////摇晃速度不变 幅度越来越大
  {19,   60,200}, {18,  120,200}, {19,  140,200}, {18,  140,200}, {19,  160,200}, {18,  160,200}, {19,  180,200}, {18,  180,200}, {19,  200,200}, 
  {18,  200,200}, {19,  220,200}, {18,  220,200}, {19,  220,200}, {18,  240,200}, {19,  260,200}, {18,  260,200}, {19,  280,200}, 
  {18,  300,200}, {19,  300,200}, {18,  320,200}, {19,  340,200}, {18,  340,200}, {19,  360,200}, {18,  360,200}, {19,  360,200}, 
  {18,  400,200}, {19,  440,200}, {18,  500,200}, {19,  500,200}, {18,  560,200}, {19,  600,200}, {18,  600,200}, {19,  600,200}, 
  {18,  660,200}, {19,  660,200}, {18,  720,200}, {19,  720,200}, {18,  780,200}, {19,  780,200}, {18,  840,200}, {19,  840,200}, 
  { 0,    0,100}, { 1,    0,100}, { 2,    0,100}, { 3,    0,100}, { 4,    0,100}, { 5,    0,100}, { 6,    0,100}, { 7,    0,100}, { 8, 2000,100}, 
  // // WAVE:  T/2 = 1920ms, T_90deg = 960ms, T_delta = 240ms
  {17,  360, 96}, {16,  360, 96}, {15,  360, 96}, {14,  360, 96}, {13,  360, 96}, {12,  360, 96}, {11,  360, 96}, {10,  360, 96}, { 9,  360, 96},
  {20,  360, 96}, {21,  360, 96}, {22,  360, 96}, {23,  360, 96}, {24,  360, 96}, {25,  360, 96}, {26,  360, 96}, {27,  360, 96}, {28,  360, 96},

  {17,  280, 96}, {16,  280, 96}, {15,  280, 96}, {14,  280, 96}, {13,  280, 96}, {12,  280, 96}, {11,  280, 96}, {10,  280, 96}, { 9,  280, 96},
  {20,  280, 96}, {21,  280, 96}, {22,  280, 96}, {23,  280, 96}, {24,  280, 96}, {25,  280, 96}, {26,  280, 96}, {27,  280, 96}, {28,  280, 96},

  {17,  200, 96}, {16,  200, 96}, {15,  200, 96}, {14,  200, 96}, {13,  200, 96}, {12,  200, 96}, {11,  200, 96}, {10,  200, 96}, { 9,  200, 96},
  {20,  200, 96}, {21,  200, 96}, {22,  200, 96}, {23,  200, 96}, {24,  200, 96}, {25,  200, 96}, {26,  200, 96}, {27,  200, 96}, {28,  200, 96},

  {17,  120, 96}, {16,  120, 96}, {15,  120, 96}, {14,  120, 96}, {13,  120, 96}, {12,  120, 96}, {11,  120, 96}, {10,  120, 96}, { 9,  120, 96},
  {20,  120, 96}, {21,  120, 96}, {22,  120, 96}, {23,  120, 96}, {24,  120, 96}, {25,  120, 96}, {26,  120, 96}, {27,  120, 96}, {28,  120, 96},

  {17,   80, 96}, {16,   80, 96}, {15,   80, 96}, {14,   80, 96}, {13,   80, 96}, {12,   80, 96}, {11,   80, 96}, {10,   80, 96}, { 9,   80, 96},
  {20,   80, 96}, {21,   80, 96}, {22,   80, 96}, {23,   80, 96}, {24,   80, 96}, {25,   80, 96}, {26,   80, 96}, {27,   80, 96}, {28,   80, 96},

  {17,  120, 96}, {16,  120, 96}, {15,  120, 96}, {14,  120, 96}, {13,  120, 96}, {12,  120, 96}, {11,  120, 96}, {10,  120, 96}, { 9,  120, 96},
  {20,  120, 96}, {21,  120, 96}, {22,  120, 96}, {23,  120, 96}, {24,  120, 96}, {25,  120, 96}, {26,  120, 96}, {27,  120, 96}, {28, 1500, 96},
  ////全部角度归零(横躺) 标准速度
  { 0,    0,100}, { 1,    0,100}, { 2,    0,100}, { 3,    0,100}, { 4,    0,100}, { 5,    0,100}, { 6,    0,100}, { 7,    0,100}, { 8, 3000,100}, 
  // ////顺时针正序挨个竖起 但不等前一个完全竖起 下一个就要开始动作 标准速度
  {17,  130,200}, {16,  130,200}, {15,  130,200}, {14,  130,200}, {13,  130,200}, {12,  130,200}, {11,  130,200}, {10,  130,200}, {9,  130,200},
};


//舵机对象列表
//每个列表项代表一个舵机 里面包含了描述舵机关键属性/状态的变量
ServoObj theServos[SERVO_SUM_MAX] = 
{//设定每个舵机的初始状态
  {  1,   0,  0 }, //1#舵机
  {  2,   0,  0 }, //2#舵机
  {  3,   0,  0 }, //3#舵机
  {  4,   0,  0 }, //4#舵机
  {  5,   0,  0 }, //5#舵机
  {  6,   0,  0 }, //6#舵机
  {  7,   0,  0 }, //7#舵机
  {  8,   0,  0 }, //8#舵机
  {  9,   0,  0 }, //9#舵机
};
//串口发送指定数据，然后接收数据
void serial_write_read(uint8_t* recv, uint8_t rsz, uint8_t* send, uint8_t ssz)
{
  delayMicroseconds(50);

  for(uint8_t i=0; i<ssz; i++)
  {
    Serial.write(send[i]);
    Serial.flush();
  }


  Serial.readBytes(recv, rsz);
}
//初始化串口设备
void init_serial_port(void)
{
  Serial.begin(115200, SERIAL_8N1);
  Serial.setTimeout(5); //5ms
}
void init_servos(void)
{//读回每个舵机的当前角度 设置每个舵机的角度校正参数

}
//flash 加载动作数据到 ram
void load_animation_unit( AnimationUnit* ram, AnimationUnit* flash)
{
  memcpy_P((void*)ram, (PGM_VOID_P)flash, sizeof(AnimationUnit));
}
//flash 加载基础动作库某个动作数据到 ram中
//motion_id - 动作ID
//ram       - 接收数据的内存地址
//srvsum    - 读取几个舵机动作数据 这个值必须要小于等于最大舵机数量 SERVO_SUM_MAX
void load_servo_motion_lib(int motion_id, ServoMotion* ram, int srvsum)
{
  ServoMotion* src = (ServoMotion*)&servo_motion_lib[motion_id][0];
  memcpy_P((void*)ram, (PGM_VOID_P)src, srvsum*sizeof(ServoMotion));
}
//计算校验和
uint8_t checksum(uint8_t* buf, uint8_t sz)
{
  uint16_t sum = 0;
  for(uint8_t i=0; i<sz; i++)
    sum += buf[i];
  return (uint8_t)(sum&0xff);
}
//给指定的舵机发送动作命令
//servo  -  舵机对象
//motion -  动作参数
//speed  -  变速因子 解释参见结构体声明 AnimationUnit
int send_servo_motion(ServoObj* servo, ServoMotion* motion, uint8_t speed)
{
  //舵机命令组包缓冲区
  static uint8_t cmd[10] = { 0xFA, 0xAF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xED };

  uint8_t srvid = servo->id;
  if((RELA==motion->type)&&(0==motion->angle))
  {//在舵机当前角度基础上偏移0度， 即动作为空 直接返回
    return 0;
  }
  if(RELA==motion->type)
  {//相对角度 先读取舵机当前角度
    cmd[2] = srvid;
    cmd[3] = 0x02;  //角度回读
    cmd[4] = 0x00;  
    cmd[5] = 0x00; 
    cmd[6] = 0x00;  
    cmd[7] = 0x00;  
    cmd[8] = checksum(&cmd[2], 6);
    serial_write_read(cmd, 10, cmd, 10); //发送10字节 接收10字节
    servo->angle  = cmd[7];              //保存读回的角度值
    servo->angle += motion->angle;
  }
  else
  {
    servo->angle = motion->angle;    //直接指定角度值
  } 
  if(servo->angle<0)
  {
    servo->angle = 0;
  }
  //发送命令 调整角度
  cmd[2] = srvid;
  cmd[3] = 0x01;                      //设置角度
  cmd[4] = servo->angle&0xFF;         //最终的目标角度
  {//运动时间
    uint32_t v = motion->time;
    v *= speed;
    v /= (100*20);
    cmd[5] = v&0xFF;
  }  
  cmd[6] = 0x00;  
  cmd[7] = 0x00;  
  cmd[8] = checksum(&cmd[2], 6);
  serial_write_read(&cmd[3], 1, cmd, 10);//发送10字节 接收1字节

  return 0;
}
//舵机demo的驱动函数 
//实际上是一个动作序列解释器
//负责将序列中的配置数据转换成一条条具体的总线舵机控制命令
//mode - 演示模式,  0 - 循环播放
//                 1 - 只播放一次 
//return ： 0 - 演示正常结束
//          1 - 异常出错 意外中止
int animation_start( int mode, ServoObj* srvlst, int srvsum, AnimationUnit* animate, int animate_len )
{
  int step   = 0; //执行到哪个动作了

  AnimationUnit au;
  ServoMotion   srvmts[SERVO_SUM_MAX]; //用于基础动作库数据的暂存

  do{
    while(step<animate_len)
    {
      load_animation_unit(&au, &animate[step]);         //从flash读取动作数据到ram
      load_servo_motion_lib(au.mid, srvmts, srvsum);    //根据基础动作ID，从动作库中加载指定数量的舵机数据
      //给每个舵机发送动作命令
      for(int i=0; i<srvsum; i++)
      {
        if(send_servo_motion(&srvlst[i], &srvmts[i], au.speed))
        {//返回非0值，表示命令发送出错
          return 1;
        }
      }
      delay_ms(au.delay);   //按照动作的要求延时
      step++;               //指向下一个动作
    }
    //动作序列执行完成 重置状态
    step  = 0;
  }while(DEMO_MODE_CYCLE==mode);

  return 0; //demo演示完成
}

void setup() 
{
  init_serial_port();
  //获取各个舵机当前角度，修正每个舵机的角度校正值
  init_servos(); 
}

void loop() 
{
  static int stop = 0; //是否停止执行demo
  if(!stop)
  {
    int ret = animation_start
    (
      DEMO_MODE, theServos, SERVO_SUM, (AnimationUnit*)servo_demo, sizeof(servo_demo)/sizeof(AnimationUnit)
    );
    if((1==ret)||(DEMO_MODE_ONCE==DEMO_MODE))
    {
      stop = 1; //中止下次循环
    }
  }
}
